/**
  
  Arduino program for controlling servos and reading a2d convertors.
  
  Companion ROS Node TK.
  
  Input Format
    [S|A][Pin#]{VALUE}
    
*/

#include <Servo.h>

Servo servo0;
Servo servo1; 
Servo servo2;

// PWM DIO pins
int servo0_pin = 9;
int servo1_pin = 10;
int servo2_pin = 11;

int sensor_val = 0;
int servo_pos = 0;

void setup()
{
  // Config serial port
  Serial.begin(115200);
  
  // Init servo pins
  servo0.attach(servo0_pin);  
  servo1.attach(servo1_pin);  
  servo2.attach(servo2_pin);  
  
  // Set all servos to center
  servo0.write(90);
  servo1.write(90);
  servo2.write(90);
  
}

void loop()
{
  // Command packet is 5 Bytes
  if ( Serial.available() >= 5 )
  {
    // Read the 'Packet'
    char cmd = Serial.read();
    char dev_num = Serial.read();
    char int_arg0 = Serial.read();
    char int_arg1 = Serial.read();
    char int_arg2 = Serial.read();
    
    // Accumulate the command value
    int cmd_val = 100 * (int_arg0 - '0') +
                  10 * (int_arg1 - '0') +
                  (int_arg2 - '0');
                  
    // If this is a servo command, set the the servo                   
    if(cmd == 's' || cmd == 'S') 
    {
      // Set the servo
      switch (dev_num) {
        case '0':
          servo0.write(cmd_val);
          break;
        case '1':
          servo1.write(cmd_val);
          break;
        case '2':
          servo2.write(cmd_val);
          break;
        default:
          break;          
      }
    }
    
    // If this is an A2D read, read and return   
    else if(cmd == 'a' || cmd == 'A')
    {         
      int sensor_val = analogRead( dev_num );
      Serial.println(sensor_val);
    }
    
    Serial.flush();
  } 
}

